확장 칼만 필터(EKF)

1. 칼만 필터의 한계와 확장 칼만 필터의 탄생

1.1 되짚어보기: 선형 칼만 필터(KF)의 완벽한 세계

선형 칼만 필터(Linear Kalman Filter, KF)는 ‘추정(Estimation)’ 문제에 있어 거의 표준과 같은 알고리즘이다. 하지만 이 강력한 도구는 ’선형(Linear)’이라는 엄격한 조건이 붙은 세계에서만 최적의 성능을 발휘한다.1 선형 칼만 필터의 핵심은 두 가지 단계, 예측(Prediction)과 업데이트(Update)의 끝없는 재귀적 반복에 있다.

• 예측 단계: 시스템의 동역학 모델(dynamic model)을 사용하여 이전 상태로부터 현재 상태를 추정한다. 예를 들어, “이전 위치와 속도를 보니, 1초 뒤에는 저기쯤 가 있겠군” 하고 예측하는 것이다. 이 단계에서는 시스템의 불확실성(프로세스 노이즈) 때문에 추정치의 불확실성(오차 공분산)이 증가한다.
• 업데이트 단계: 센서로부터 실제 측정값을 받아서, 예측이 얼마나 빗나갔는지 확인하고 이를 보정한다. “예측은 저기였는데, 센서를 보니 실제로는 여기네. 예측과 측정값 사이의 적절한 지점으로 최종 추정치를 수정해야겠다“고 결정하는 것이다. 이 과정에서 외부 정보를 얻었기 때문에 추정치의 불확실성은 다시 감소한다.2

이 과정이 재귀적(recursive)이라는 점은 칼만 필터의 가장 큰 효율성 중 하나다. 과거의 모든 데이터를 저장하고 매번 다시 계산할 필요 없이, 바로 직전 스텝의 추정값(상태와 공분산)만 있으면 현재 상태를 추정할 수 있다.3 이는 메모리 사용량이 적고 계산 속도가 빨라 실시간 시스템에 매우 적합하다는 의미다.

하지만 이 모든 아름다운 과정은 두 가지 핵심적인 가정 위에 세워져 있다. 첫째, 시스템의 움직임(상태 전이)과 측정 과정이 모두 행렬 곱으로 표현되는 선형 관계여야 한다(\mathbf{x}_k = \mathbf{A}\mathbf{x}_{k-1} + \mathbf{B}\mathbf{u}_{k-1}, \mathbf{z}_k = \mathbf{H}\mathbf{x}_k). 둘째, 시스템 모델의 불확실성(프로세스 노이즈)과 측정값의 불확실성(측정 노이즈)이 모두 평균이 0인 가우시안 분포(정규 분포)를 따라야 한다.1

1.2 현실의 벽: 비선형성이 가우시안 분포를 파괴할 때

문제는 현실 세계가 선형적이지 않다는 점이다.1 자율주행 자동차가 코너를 도는 움직임, 로봇팔이 관절을 꺾어 물체를 집는 동작, 심지어 레이더 센서가 물체의 위치를 측정하는 방식까지, 그 어떤 것도 완벽한 선형 관계로 설명되지 않는다. 예를 들어, 레이더는 물체까지의 거리(\rho)와 방위각(\phi)을 측정하는데, 이를 직교 좌표계의 위치 (p_x, p_y)로 변환하려면 \rho = \sqrt{p_x^2 + p_y^2}나 \phi = \operatorname{atan2}(p_y, p_x)와 같은 삼각함수와 제곱근이 포함된 비선형 함수가 필요하다.5

선형 칼만 필터의 수학적 아름다움과 최적성은 ’가우시안 분포의 보존’이라는 특성에서 기인한다. 가우시안 분포를 따르는 확률 변수를 선형 변환(행렬 곱)하면, 그 결과 역시 가우시안 분포를 유지한다. 덕분에 우리는 상태를 평균과 공분산만으로 완벽하게 표현하고 추적할 수 있다.4 하지만 이 가우시안 분포가 비선형 함수를 통과하는 순간, 그 결과는 더 이상 아름다운 종 모양의 가우시안 분포가 아니라, 찌그러지고 비대칭적인 복잡한 형태의 분포로 변해버린다.4 이렇게 되면 평균과 공분산만으로는 상태의 불확실성을 제대로 표현할 수 없게 되고, 선형 칼만 필터의 수학적 기반이 송두리째 흔들리게 된다.6 이것이 바로 확장 칼만 필터가 해결해야만 하는 근본적인 문제다.

1.3 EKF의 접근법: ‘순간의’ 선형성으로 비선형을 길들이다

확장 칼만 필터(Extended Kalman Filter, EKF)의 핵심 아이디어는 매우 직관적이다. “전체 곡선을 한 번에 다룰 수 없다면, 우리가 관심 있는 한 지점 근처에서 아주 짧은 직선으로 잘라내어 근사하자“는 것이다.1 EKF는 다루기 힘든 비선형 함수를 현재 상태 추정치라는 특정 지점에서 ’테일러 급수 전개(Taylor Series Expansion)’를 이용해 1차 선형 함수, 즉 접선으로 근사한다.7 그리고 이 근사된 선형 모델을 가지고 기존 선형 칼만 필터의 예측 및 업데이트 메커니즘을 거의 그대로 적용한다.

이 ‘순간적인’ 선형화는 EKF의 가장 큰 장점이자 동시에 가장 치명적인 약점이 된다.

• 장점: 선형 칼만 필터의 강력하고 잘 정립된 프레임워크를 비선형 시스템에까지 확장하여 적용할 수 있게 해준다. 덕분에 EKF는 수십 년간 GPS, 내비게이션, 로봇 공학 등 수많은 분야에서 가장 대중적인 비선형 필터로 자리 잡았다.1
• 약점: 근사는 결국 근사일 뿐이다. 실제 비선형 모델과 선형화된 모델 사이의 오차(linearization error)는 필연적으로 발생한다. 이 오차 때문에 EKF는 선형 KF와 달리 더 이상 통계적으로 ’최적(optimal)’인 필터가 아니다.1 시스템의 비선형성이 강할수록, 즉 곡선이 많이 휘어있을수록 이 오차는 걷잡을 수 없이 커지고, 필터가 완전히 잘못된 추정치를 내놓으며 발산(diverge)할 위험이 커진다.8 이러한 이유로 EKF의 성능은 초기 추정값을 얼마나 정확하게 설정하느냐에 매우 민감하다. 초기 오차가 크면, 엉뚱한 지점에서 선형화를 수행하게 되고 첫 단추부터 잘못 끼워져 결국 발산으로 이어질 수 있다.

2. EKF의 수학적 심장: 자코비안 행렬

2.1 비선형 함수를 선형으로 근사하는 마법, 테일러 급수

EKF의 선형화 아이디어는 테일러 급수(Taylor Series)에서 출발한다. 어떤 미분 가능한 함수 f(x)를 특정 지점 a 근처에서 다항식의 합으로 근사하는 방법이다.

f(x) = f(a) + \frac{f'(a)}{1!}(x-a) + \frac{f''(a)}{2!}(x-a)^2 + \dots
EKF는 이 복잡한 식에서 과감하게 2차 이상의 고차항을 모두 무시하고 1차 항까지만 사용한다.7

f(x) \approx f(a) + f'(a)(x-a)
이것은 다름 아닌 점 a에서 함수 f(x)에 접하는 접선의 방정식이다. 즉, EKF는 복잡한 곡선을 아주 작은 구간에서는 직선으로 취급하겠다는 전략을 사용한다.7

2.2 자코비안 행렬: 다변수 함수의 ‘기울기 집합’

하지만 우리가 다루는 시스템의 상태는 위치, 속도 등 여러 변수로 구성된 벡터(\mathbf{x})이고, 이를 통해 예측하거나 측정하는 값 역시 여러 변수로 구성된 벡터(\mathbf{y})다. 이처럼 입력과 출력이 모두 여러 개인 다변수 함수에서는 단순한 미분계수 f'(a) 대신, 모든 입력 변수에 대한 모든 출력 함수의 편미분(partial derivative) 값을 모아놓은 행렬, 즉 자코비안 행렬(Jacobian Matrix)을 사용한다.1

예를 들어, 입력 벡터가 \mathbf{x} = [x_1, x_2]^T이고 출력 벡터가 \mathbf{y} = f(\mathbf{x}) = [y_1(x_1, x_2), y_2(x_1, x_2)]^T인 함수가 있다고 하자. 이 함수의 자코비안 행렬 \mathbf{J}는 다음과 같이 정의된다.

\mathbf{J} = \frac{\partial \mathbf{y}}{\partial \mathbf{x}} = \begin{bmatrix} \frac{\partial y_1}{\partial x_1} & \frac{\partial y_1}{\partial x_2} \\ \frac{\partial y_2}{\partial x_1} & \frac{\partial y_2}{\partial x_2} \end{bmatrix}

2.3 선형 KF의 A, H를 대체하는 동적 행렬 F, H

EKF에서 자코비안 행렬은 선형 칼만 필터의 상태 전이 행렬 \mathbf{A}와 측정 행렬 \mathbf{H}의 역할을 비선형 시스템에서 대신 수행한다. 혼동을 피하기 위해 EKF에서는 이들을 각각 \mathbf{F}와 \mathbf{H}로 표기하는 경우가 많다.1 중요한 차이점은 이들이 더 이상 고정된 상수 행렬이 아니라는 점이다. 자코비안은 선형화의 기준점, 즉 현재 상태 추정치 \hat{\mathbf{x}}에 따라 그 값이 매번 달라지는 동적인 행렬이다.3 이 때문에 EKF는 매 스텝마다 자코비안을 새로 계산해야 하는 추가적인 계산 부담을 안게 된다.11

3. 확장 칼만 필터 알고리즘 완전 해부

3.1 일반적인 비선형 시스템 모델

EKF가 다루는 비선형 시스템 모델은 일반적으로 다음과 같이 표현된다.2

\mathbf{x}_k = f(\mathbf{x}_{k-1}, \mathbf{u}_{k-1}) + \mathbf{w}_{k-1}
\mathbf{z}_k = h(\mathbf{x}_k) + \mathbf{v}_k
여기서 f는 현재 상태가 이전 상태로부터 어떻게 변하는지를 나타내는 비선형 상태 전이 함수이고, h는 현재 상태가 센서에서 어떻게 측정되는지를 나타내는 비선형 측정 함수다. \mathbf{w}_{k-1}와 \mathbf{v}_k는 각각 평균이 0이고 공분산이 \mathbf{Q}_{k-1}과 \mathbf{R}_k인 가우시안 분포를 따르는 프로세스 노이즈와 측정 노이즈를 의미한다.

3.2 예측 단계 (Prediction Step): “어디로 갈 것인가?”

예측 단계의 목표는 이전 시간(k-1)의 최종 추정치(\hat{\mathbf{x}}_{k-1|k-1}, \mathbf{P}_{k-1|k-1})를 바탕으로 현재 시간(k)의 상태(\hat{\mathbf{x}}_{k|k-1})와 그 불확실성(\mathbf{P}_{k|k-1})을 예측하는 것이다.

• 상태 예측 (State Prediction)
  \hat{\mathbf{x}}_{k|k-1} = f(\hat{\mathbf{x}}_{k-1|k-1}, \mathbf{u}_{k-1})
  상태 자체를 예측할 때는 선형화된 모델이 아닌, 실제 비선형 상태 전이 함수 f를 직접 사용한다.3 이는 선형화로 인한 오차를 상태 예측 자체에는 개입시키지 않으려는 의도이며, 가능한 한 실제 시스템의 움직임을 그대로 반영하여 더 정확한 예측을 하기 위함이다.3

• 오차 공분산 예측 (Error Covariance Prediction)

\mathbf{P}_{k|k-1} = \mathbf{F}_{k-1} \mathbf{P}_{k-1|k-1} \mathbf{F}_{k-1}^T + \mathbf{Q}_{k-1}
상태의 불확실성이 시간이 지남에 따라 어떻게 커지는지를 예측한다. 여기서는 상태 자체가 아닌, 오차의 전파를 다루기 때문에 선형화된 모델, 즉 자코비안 \mathbf{F}_{k-1}을 사용한다.3 수식의 구조는 선형 KF와 동일하며, 단지 상태 전이 행렬 \mathbf{A}가 자코비안 \mathbf{F}_{k-1}으로 대체되었을 뿐이다. 이 자코비안은 상태 전이 함수 f를 이전 스텝의 최종 추정치 \hat{\mathbf{x}}_{k-1|k-1}에 대해 편미분하여 구한다.

\mathbf{F}_{k-1} = \left. \frac{\partial f}{\partial \mathbf{x}} \right|_{\hat{\mathbf{x}}_{k-1|k-1}, \mathbf{u}_{k-1}}

3.3 업데이트 단계 (Update Step): “실제는 어떠한가?”

업데이트 단계에서는 센서로부터 들어온 실제 측정값(\mathbf{z}_k)을 사용하여 예측 단계에서 계산한 예측치(\hat{\mathbf{x}}_{k|k-1}, \mathbf{P}_{k|k-1})를 보정하고, 현재 시간(k)의 최종 추정치(\hat{\mathbf{x}}_{k|k}, \mathbf{P}_{k|k})를 계산한다.

• 혁신 또는 잔차 계산 (Innovation / Residual)

\tilde{\mathbf{y}}_k = \mathbf{z}_k - h(\hat{\mathbf{x}}_{k|k-1})
예측된 측정값과 실제 측정값 사이의 차이를 계산한다.3 이 차이를 ‘혁신(Innovation)’ 또는 ’잔차(Residual)’라고 부르는데, 이는 예측이 얼마나 ‘놀라운’ 결과를 낳았는지를 나타내기 때문이다. 상태 예측과 마찬가지로, 예측된 측정값을 계산할 때는 비선형 측정 함수

h를 직접 사용한다. h(\hat{\mathbf{x}}_{k|k-1})는 “만약 우리의 상태 예측이 맞다면, 센서는 이런 값을 보여줬을 것이다“라는 의미를 가진다.

• 혁신 공분산 계산 (Innovation Covariance)

\mathbf{S}_k = \mathbf{H}_k \mathbf{P}_{k|k-1} \mathbf{H}_k^T + \mathbf{R}_k
이 ’놀라움’이 얼마나 큰 불확실성을 가지는지를 계산한다.3 이는 두 가지 불확실성의 합으로 구성된다. 첫째는 예측된 상태의 불확실성(\mathbf{P}_{k|k-1})이 측정 공간으로 투영된 것(\mathbf{H}_k \mathbf{P}_{k|k-1} \mathbf{H}_k^T)이고, 둘째는 측정 자체의 불확실성(\mathbf{R}_k)이다. 여기서 자코비안 \mathbf{H}_k는 측정 함수 h를 현재 스텝의 예측치 \hat{\mathbf{x}}_{k|k-1}에 대해 편미분하여 구한다.

\mathbf{H}_k = \left. \frac{\partial h}{\partial \mathbf{x}} \right|_{\hat{\mathbf{x}}_{k|k-1}}
칼만 이득 계산 (Kalman Gain)
\mathbf{K}_k = \mathbf{P}_{k|k-1} \mathbf{H}_k^T \mathbf{S}_k^{-1}
칼만 이득 \mathbf{K}_k는 예측의 불확실성(\mathbf{P}_{k|k-1})과 측정의 불확실성(\mathbf{S}_k)을 종합적으로 고려하여, 우리가 계산한 ‘잔차’ \tilde{\mathbf{y}}_k를 얼마나 신뢰하고 반영할지를 결정하는 최적의 가중치다.3

• 상태 업데이트 (State Update)

\hat{\mathbf{x}}_{k|k} = \hat{\mathbf{x}}_{k|k-1} + \mathbf{K}_k \tilde{\mathbf{y}}_k
최종적으로 현재 상태를 추정한다. 예측된 상태 \hat{\mathbf{x}}_{k|k-1}에 ‘칼만 이득 × 잔차’ 만큼을 더하여 보정한다.3

• 오차 공분산 업데이트 (Error Covariance Update)
  \mathbf{P}_{k|k} = (I - \mathbf{K}_k \mathbf{H}_k) \mathbf{P}_{k|k-1}
  측정값을 통해 새로운 정보를 얻었으므로, 상태의 불확실성은 예측 단계보다 줄어들어야 한다. (I - \mathbf{K}_k \mathbf{H}_k) 항은 이 불확실성 감소 효과를 수학적으로 나타낸다.3

3.4 핵심 질문에 대한 심층 답변: 왜 F와 H의 선형화 기준점이 다른가?

여기서 한 가지 중요한 질문이 생긴다. 왜 상태 전이 자코비안 \mathbf{F}_{k-1}은 이전 스텝의 최종 추정치 \hat{\mathbf{x}}_{k-1|k-1}을 기준으로 계산하고, 측정 자코비안 \mathbf{H}_k는 현재 스텝의 예측치 \hat{\mathbf{x}}_{k|k-1}을 기준으로 계산하는 것일까? 이는 각 자코비안이 사용되는 목적과 시점을 생각해보면 명확해진다. 핵심 원리는 “각 계산이 수행되는 시점에서 가장 최신의, 그리고 가장 적절한 정보를 사용한다“는 것이다.

1. \mathbf{F}_{k-1}의 역할: 자코비안 \mathbf{F}_{k-1}은 k-1 시점에서 k 시점으로의 오차 공분산을 예측하는 데 사용된다 (\mathbf{P}_{k|k-1} = \mathbf{F}_{k-1} \mathbf{P}_{k-1|k-1} \mathbf{F}_{k-1}^T + \mathbf{Q}_{k-1}). 이 계산은 k 시점의 측정값이 들어오기 전에 수행된다. 이 시점에서 우리가 가진 가장 정확하고 보정된 상태 정보는 바로 k-1 시점의 모든 정보를 종합하여 얻은 최종 추정치, 즉 \hat{\mathbf{x}}_{k-1|k-1}이다. 따라서 이 지점에서 운동 모델 f를 선형화하여 불확실성을 전파시키는 것이 가장 합리적인 선택이다.3
2. \mathbf{H}_k의 역할: 자코비안 \mathbf{H}_k는 k 시점의 측정값을 이용해 예측을 보정하는 업데이트 단계에서 사용된다. 이 단계의 핵심은 실제 측정값 \mathbf{z}_k와 예측된 측정값 h(\hat{\mathbf{x}}_{k|k-1})을 비교하는 것이다. 자코비안 \mathbf{H}_k는 상태 예측치 \hat{\mathbf{x}}_{k|k-1}의 작은 변화가 예측된 측정값 h(\hat{\mathbf{x}}_{k|k-1})에 어떤 영향을 미치는지를 나타내는 선형 관계를 모델링한다. 따라서 이 둘 사이의 관계를 가장 잘 나타내는 지역적 선형 모델을 얻기 위해서는, 당연히 예측치 \hat{\mathbf{x}}_{k|k-1}를 기준으로 측정 함수 h를 선형화해야 한다.3

결론적으로, 각 자코비안은 해당 계산이 수행되는 시점에서 가장 최신의, 그리고 가장 적절한 상태 추정치를 기준으로 선형화된다. 이는 EKF가 매 스텝마다 변화하는 상황에 맞춰 ‘최선의’ 선형 모델을 동적으로 찾아가려는 노력의 일환이라고 볼 수 있다.

4. 실전 예제 1: CTRV 모델을 이용한 차량 추적

이제 이론을 실제 문제에 적용해 보자. 자율주행이나 로봇 공학에서 가장 흔하게 접하는 문제 중 하나는 움직이는 차량을 추적하는 것이다. 이를 위해 널리 사용되는 CTRV(Constant Turn Rate and Velocity) 모델을 EKF에 적용하는 과정을 단계별로 살펴보자.

4.1 CTRV (Constant Turn Rate and Velocity) 모델 소개

CTRV 모델은 이름에서 알 수 있듯이, 아주 짧은 시간 간격(\Delta t) 동안에는 차량의 선회율(turn rate, \dot{\psi})과 속력(velocity, v)이 일정하다고 가정하는 운동 모델이다.12 이는 단순히 등속 직선 운동(Constant Velocity, CV)을 가정하는 것보다 회전하는 차량의 움직임을 훨씬 더 현실적으로 묘사할 수 있어 널리 사용된다.

• 상태 벡터 정의: CTRV 모델에서 우리가 추적하고자 하는 객체의 상태는 보통 5개의 변수로 구성된 상태 벡터 \mathbf{x}로 정의한다.12
• p_x, p_y: 차량의 전역 좌표계(Global Frame) 상에서의 x, y 위치
• v: 차량의 속력 (방향이 아닌 스칼라 값)
• \psi: 차량의 주행 방향 각도 (Yaw 또는 Heading)
• \dot{\psi}: 차량의 각속도, 즉 선회율 (Yaw Rate)
• \mathbf{x} = [p_x, p_y, v, \psi, \dot{\psi}]^T

4.2 상태 전이 함수 f(x) 단계별 유도

상태 전이 함수 f(\mathbf{x})는 k-1 시점의 상태 \mathbf{x}_{k-1}이 주어졌을 때, \Delta t 시간이 흐른 뒤의 k 시점의 상태 \mathbf{x}_k를 예측하는 비선형 함수다. 각 상태 변수가 어떻게 변하는지 살펴보자.

속력 v, 주행 방향 \psi, 선회율 \dot{\psi}의 업데이트는 CTRV 가정에 따라 매우 간단하다.

• v_k = v_{k-1} (속력은 일정)
• \dot{\psi}_k = \dot{\psi}_{k-1} (선회율은 일정)
• \psi_k = \psi_{k-1} + \dot{\psi}_{k-1} \Delta t (주행 방향은 선회율에 따라 변함)

위치 p_x, p_y의 업데이트는 적분을 통해 계산해야 한다. 시간 t에서의 위치 변화율은 \dot{p}_x(t) = v(t) \cos(\psi(t)), \dot{p}_y(t) = v(t) \sin(\psi(t))이다. CTRV 가정에 따라 v(t) = v_{k-1}이고 \psi(t) = \psi_{k-1} + \dot{\psi}_{k-1}(t-t_{k-1})이므로, 이를 t_{k-1}부터 t_k까지 \Delta t 동안 적분하면 된다.12

4.2.1 Case 1: \dot{\psi}_{k-1} \neq 0 (회전 중일 때)

선회율이 0이 아닐 때는 삼각함수의 적분을 통해 다음과 같은 식을 얻는다.12

\begin{align*} p_{x,k} &= p_{x,k-1} + \int_{t_{k-1}}^{t_k} v_{k-1} \cos(\psi_{k-1} + \dot{\psi}_{k-1}(t-t_{k-1})) dt \\ &= p_{x,k-1} + \left[ \frac{v_{k-1}}{\dot{\psi}_{k-1}} \sin(\psi_{k-1} + \dot{\psi}_{k-1}(t-t_{k-1})) \right]_{t_{k-1}}^{t_k} \\ &= p_{x,k-1} + \frac{v_{k-1}}{\dot{\psi}_{k-1}} (\sin(\psi_{k-1} + \dot{\psi}_{k-1}\Delta t) - \sin(\psi_{k-1})) \end{align*}

$$
$$
\begin{align*}
p_{y,k} &= p_{y,k-1} + \int_{t_{k-1}}^{{t_k} v_{k-1} \sin(\psi_{k-1} + \dot{\psi}{k-1}(t-t{k-1})) dt \
&= p_{y,k-1} + \left[ -\frac{v_{k-1}}{\dot{\psi}{k-1}} \cos(\psi{k-1} + \dot{\psi}{k-1}(t-t{k-1})) \right]{t{k-1}}}{t_k} \
&= p_{y,k-1} + \frac{v_{k-1}}{\dot{\psi}{k-1}} (-\cos(\psi{k-1} + \dot{\psi}{k-1}\Delta t) + \cos(\psi{k-1}))
\end{align*}
$$

Case 2: \dot{\psi}_{k-1} = 0 (직진 중일 때)

위 식에서 \dot{\psi}_{k-1}이 0에 가까워지면 분모가 0이 되어 수치적으로 불안정해지는 문제가 발생한다. 이는 단순히 수학적 특이점(singularity)일 뿐만 아니라 물리적 현실을 반영한다. 선회율이 0이라는 것은 차량이 회전하지 않고 직선 운동을 한다는 의미다. 이 경우, \psi(t)는 \psi_{k-1}로 상수이므로 적분이 훨씬 간단해진다.12

\begin{align*} p_{x,k} &= p_{x,k-1} + \int_{t_{k-1}}^{t_k} v_{k-1} \cos(\psi_{k-1}) dt = p_{x,k-1} + v_{k-1} \cos(\psi_{k-1}) \Delta t \\ p_{y,k} &= p_{y,k-1} + \int_{t_{k-1}}^{t_k} v_{k-1} \sin(\psi_{k-1}) dt = p_{y,k-1} + v_{k-1} \sin(\psi_{k-1}) \Delta t \end{align*}
실제 코드를 구현할 때는 if abs(yaw_rate) < 0.0001: 과 같이 아주 작은 임계값을 설정하여 이 두 경우를 분기 처리하는 것이 필수적이다. 이 분기 처리는 수학적 오류를 피하는 것뿐만 아니라, 물리적으로 다른 두 운동 모델(원운동과 직선운동)을 올바르게 전환하는 과정이다.12

상태 전이 자코비안 F 단계별 유도

이제 EKF 구현의 가장 까다로운 부분인 자코비안 \mathbf{F}_k를 유도할 차례다. \mathbf{F}_k는 상태 전이 함수 f(\mathbf{x})의 5개 출력 함수(p_{x,k}, p_{y,k}, v_k, \psi_k, \dot{\psi}_k)를 5개의 입력 상태 변수(p_{x,k-1}, p_{y,k-1}, v_{k-1}, \psi_{k-1}, \dot{\psi}_{k-1}) 각각에 대해 편미분한 5x5 행렬이다.15

\mathbf{F}_k = \left. \frac{\partial f}{\partial \mathbf{x}} \right|_{\hat{\mathbf{x}}_{k-1|k-1}}
대부분의 항은 미분하면 0, 1, 또는 \Delta t가 되어 간단하다. 예를 들어, \frac{\partial p_{x,k}}{\partial p_{x,k-1}} = 1, \frac{\partial \psi_k}{\partial \psi_{k-1}} = 1, \frac{\partial \psi_k}{\partial \dot{\psi}_{k-1}} = \Delta t 이다. 복잡한 부분은 위치 p_x, p_y를 속도, 주행 방향, 선회율(v, \psi, \dot{\psi})로 편미분하는 항들이다.

Case 1: \dot{\psi}_{k-1} \neq 0 일 때의 \mathbf{F}_k

편의상 v = v_{k-1}, \psi = \psi_{k-1}, \dot{\psi} = \dot{\psi}_{k-1} 라고 간단히 표기하자.

• \frac{\partial p_{x,k}}{\partial v} = \frac{1}{\dot{\psi}} (\sin(\psi + \dot{\psi}\Delta t) - \sin(\psi))
• \frac{\partial p_{x,k}}{\partial \psi} = \frac{v}{\dot{\psi}} (\cos(\psi + \dot{\psi}\Delta t) - \cos(\psi))
• \frac{\partial p_{x,k}}{\partial \dot{\psi}} = \frac{v\Delta t}{\dot{\psi}} \cos(\psi + \dot{\psi}\Delta t) - \frac{v}{\dot{\psi}^2} (\sin(\psi + \dot{\psi}\Delta t) - \sin(\psi))
• \frac{\partial p_{y,k}}{\partial v} = \frac{1}{\dot{\psi}} (-\cos(\psi + \dot{\psi}\Delta t) + \cos(\psi))
• \frac{\partial p_{y,k}}{\partial \psi} = \frac{v}{\dot{\psi}} (\sin(\psi + \dot{\psi}\Delta t) - \sin(\psi))
• \frac{\partial p_{y,k}}{\partial \dot{\psi}} = \frac{v\Delta t}{\dot{\psi}} \sin(\psi + \dot{\psi}\Delta t) - \frac{v}{\dot{\psi}^2} (-\cos(\psi + \dot{\psi}\Delta t) + \cos(\psi))

이를 모두 모으면 자코비안 \mathbf{F}_k는 다음과 같다.

\mathbf{F}_k = \begin{bmatrix} 1 & 0 & \frac{1}{\dot{\psi}}(\sin(\psi+\dot{\psi}\Delta t) - \sin(\psi)) & \frac{v}{\dot{\psi}}(\cos(\psi+\dot{\psi}\Delta t) - \cos(\psi)) & \frac{\partial p_{x,k}}{\partial \dot{\psi}} \\ 0 & 1 & \frac{1}{\dot{\psi}}(-\cos(\psi+\dot{\psi}\Delta t) + \cos(\psi)) & \frac{v}{\dot{\psi}}(\sin(\psi+\dot{\psi}\Delta t) - \sin(\psi)) & \frac{\partial p_{y,k}}{\partial \dot{\psi}} \\ 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 1 & \Delta t \\ 0 & 0 & 0 & 0 & 1 \end{bmatrix}

Case 2: \dot{\psi}_{k-1} = 0 일 때의 \mathbf{F}_k

직진 운동 시에는 상태 전이 함수가 더 단순하므로 자코비안도 간단해진다.

• \frac{\partial p_{x,k}}{\partial v} = \cos(\psi)\Delta t
• \frac{\partial p_{x,k}}{\partial \psi} = -v\sin(\psi)\Delta t
• \frac{\partial p_{y,k}}{\partial v} = \sin(\psi)\Delta t
• \frac{\partial p_{y,k}}{\partial \psi} = v\cos(\psi)\Delta t
• \dot{\psi}에 대한 편미분 항들은 0이 된다. 하지만 0으로 나누는 특이점을 피하기 위해 \dot{\psi} \to 0 극한을 취하면, \frac{\partial p_{x,k}}{\partial \dot{\psi}}와 \frac{\partial p_{y,k}}{\partial \dot{\psi}} 항은 0이 아닌 값을 가진다. 로피탈의 정리를 적용하면 \lim_{\dot{\psi} \to 0} \frac{\partial p_{x,k}}{\partial \dot{\psi}} = -\frac{1}{2}v\Delta t^2 \sin(\psi) 와 \lim_{\dot{\psi} \to 0} \frac{\partial p_{y,k}}{\partial \dot{\psi}} = \frac{1}{2}v\Delta t^2 \cos(\psi)를 얻을 수 있다. 하지만 많은 간소화된 구현에서는 이 항들을 0으로 처리하기도 한다. 여기서는 더 정확한 모델을 위해 0으로 두지 않는다.

이때의 자코비안은 다음과 같다.

\mathbf{F}_k = \begin{bmatrix} 1 & 0 & \cos(\psi)\Delta t & -v\sin(\psi)\Delta t & -\frac{1}{2}v\Delta t^2 \sin(\psi) \\ 0 & 1 & \sin(\psi)\Delta t & v\cos(\psi)\Delta t & \frac{1}{2}v\Delta t^2 \cos(\psi) \\ 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 1 & \Delta t \\ 0 & 0 & 0 & 0 & 1 \end{bmatrix}
이처럼 자코비안을 손으로 직접 유도하는 과정은 EKF 구현에서 가장 까다롭고 버그가 발생하기 쉬운 부분이다. 사소한 부호 실수나 미분 계산 착오가 필터 전체의 성능을 망가뜨릴 수 있다. 바로 이 점이 많은 개발자들이 자코비안 계산이 필요 없는 무향 칼만 필터(UKF)를 선호하게 되는 주된 이유 중 하나다.18 하지만 이 과정을 상세히 따라가 보는 것은 EKF의 실제적인 어려움과 그 작동 원리를 깊이 이해하는 데 필수적인 학습 과정이다.

실전 예제 2: 라이다(Lidar)와 레이더(Radar) 센서 퓨전

이제 EKF의 또 다른 핵심 응용 분야인 센서 퓨전(Sensor Fusion)을 살펴보자. 자율주행차는 보통 라이다, 레이더, 카메라 등 여러 종류의 센서를 함께 사용하는데, 각 센서의 장단점을 보완하여 더 강건하고 정확한 인지를 하기 위함이다. EKF는 이처럼 서로 다른 종류의 측정값을 하나의 일관된 상태 추정치로 통합하는 데 매우 효과적인 도구다.

측정 모델 h(x)의 역할

측정 모델 h(\mathbf{x})는 추상적인 상태 공간(State Space)의 변수들을 센서가 실제로 측정하는 물리적인 측정 공간(Measurement Space)으로 변환하는 다리 역할을 한다.19 예를 들어, CTRV 모델의 상태 벡터 \mathbf{x} = [p_x, p_y, v, \psi, \dot{\psi}]^T를 레이더가 측정하는 (\rho, \phi, \dot{\rho}) 값으로 변환하는 함수가 바로 레이더의 측정 모델 h_{Radar}(\mathbf{x})이다.

라이다 측정 모델과 자코비안 \mathbf{H}_{Lidar}

라이다(Lidar)는 레이저 펄스를 이용해 주변 환경을 스캔하고, 매우 정밀한 3D 포인트 클라우드를 생성한다. 차량 추적의 맥락에서, 라이다는 보통 객체의 직교 좌표계 위치 (p_x, p_y)를 직접적으로 측정한다고 모델링할 수 있다.

• 측정 벡터: \mathbf{z}_{Lidar} = [p_x, p_y]^T

• 측정 함수: 상태 벡터 \mathbf{x}에서 위치 정보인 p_x와 p_y를 그대로 가져오면 된다. 이는 선형 변환으로 표현할 수 있다.

h_{Lidar}(\mathbf{x}) = \begin{bmatrix} p_x \\ p_y \end{bmatrix} = \begin{bmatrix} 1 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 \end{bmatrix} \mathbf{x}

• 자코비안 \mathbf{H}_{Lidar}: 측정 함수가 이미 선형이므로, 이를 상태 벡터 \mathbf{x}에 대해 편미분한 자코비안은 상수 행렬이 된다.

\mathbf{H}_{Lidar} = \frac{\partial h_{Lidar}}{\partial \mathbf{x}} = \begin{bmatrix} 1 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 \end{bmatrix}

라이다의 측정 모델은 선형이기 때문에, 라이다 측정값으로 업데이트를 수행할 때는 EKF가 사실상 선형 칼만 필터처럼 동작한다. 비선형성은 시스템의 움직임을 예측하는 단계와, 아래에서 다룰 비선형적인 레이더 측정값을 처리하는 단계에서만 나타난다.

레이더 측정 모델(\rho, \phi, \dot{\rho})과 자코비안 \mathbf{H}_{Radar} 단계별 유도

레이더(Radar)는 전파를 이용하여 객체를 탐지하며, 보통 극좌표계(polar coordinates)로 측정값을 제공한다: 센서로부터 객체까지의 거리(range, \rho), 방위각(bearing, \phi), 그리고 거리가 변하는 속도(range rate, \dot{\rho}).5 이 측정값들과 상태 변수 (p_x, p_y, v, \psi) 사이의 관계는 비선형적이다.

• 측정 벡터: \mathbf{z}_{Radar} = [\rho, \phi, \dot{\rho}]^T

• 측정 함수 h_{Radar}(\mathbf{x}): 상태 벡터 \mathbf{x}를 레이더 측정 벡터 \mathbf{z}_{Radar}로 변환하는 함수는 다음과 같다.

h_{Radar}(\mathbf{x}) = \begin{bmatrix} \rho \\ \phi \\ \dot{\rho} \end{bmatrix} = \begin{bmatrix} \sqrt{p_x^2 + p_y^2} \\ \operatorname{atan2}(p_y, p_x) \\ \frac{p_x v \cos(\psi) + p_y v \sin(\psi)}{\sqrt{p_x^2 + p_y^2}} \end{bmatrix}
여기서 \dot{\rho} 유도 과정은 다음과 같다. 위치 벡터 \mathbf{p} = [p_x, p_y]^T와 속도 벡터 \mathbf{v}_{xy} = [v\cos(\psi), v\sin(\psi)]^T의 내적을 거리 \rho로 나눈 값, 즉 속도 벡터를 위치 벡터 방향으로 투영(projection)한 크기다. 이는 \rho^2 = p_x^2 + p_y^2를 시간에 대해 미분하여 유도할 수 있다: 2\rho\dot{\rho} = 2p_x\dot{p}_x + 2p_y\dot{p}_y. 따라서 \dot{\rho} = (p_x\dot{p}_x + p_y\dot{p}_y)/\rho 이고, \dot{p}_x와 \dot{p}_y에 CTRV 모델의 속도 성분을 대입하면 위 식을 얻는다.5

• 자코비안 \mathbf{H}_{Radar}: 이제 측정 함수 h_{Radar}를 5개의 상태 변수(p_x, p_y, v, \psi, \dot{\psi}) 각각에 대해 편미분하여 3x5 크기의 자코비안 행렬 \mathbf{H}_{Radar}를 구해야 한다. 이 과정은 연쇄 법칙(chain rule)과 분수, 제곱근, 아크탄젠트 함수의 미분 등 다소 복잡한 계산을 요구한다.5

\mathbf{H}_{Radar} = \frac{\partial h_{Radar}}{\partial \mathbf{x}} = \begin{bmatrix} \frac{\partial \rho}{\partial p_x} & \frac{\partial \rho}{\partial p_y} & \frac{\partial \rho}{\partial v} & \frac{\partial \rho}{\partial \psi} & \frac{\partial \rho}{\partial \dot{\psi}} \\ \frac{\partial \phi}{\partial p_x} & \frac{\partial \phi}{\partial p_y} & \frac{\partial \phi}{\partial v} & \frac{\partial \phi}{\partial \psi} & \frac{\partial \phi}{\partial \dot{\psi}} \\ \frac{\partial \dot{\rho}}{\partial p_x} & \frac{\partial \dot{\rho}}{\partial p_y} & \frac{\partial \dot{\rho}}{\partial v} & \frac{\partial \dot{\rho}}{\partial \psi} & \frac{\partial \dot{\rho}}{\partial \dot{\psi}} \end{bmatrix}
각 항을 계산해 보면 다음과 같다. (편의상 \rho = \sqrt{p_x^2+p_y^2}로 표기)

• \frac{\partial \rho}{\partial p_x} = \frac{p_x}{\rho}, \frac{\partial \rho}{\partial p_y} = \frac{p_y}{\rho}, \frac{\partial \rho}{\partial v} = 0, \frac{\partial \rho}{\partial \psi} = 0, \frac{\partial \rho}{\partial \dot{\psi}} = 0
• \frac{\partial \phi}{\partial p_x} = -\frac{p_y}{\rho^2}, \frac{\partial \phi}{\partial p_y} = \frac{p_x}{\rho^2}, \frac{\partial \phi}{\partial v} = 0, \frac{\partial \phi}{\partial \psi} = 0, \frac{\partial \phi}{\partial \dot{\psi}} = 0
• \frac{\partial \dot{\rho}}{\partial p_x} = \frac{v\cos(\psi)}{\rho} - \frac{p_x(p_x v\cos(\psi) + p_y v\sin(\psi))}{\rho^3} = \frac{v}{\rho^3}(p_y^2 \cos\psi - p_x p_y \sin\psi)
• \frac{\partial \dot{\rho}}{\partial p_y} = \frac{v\sin(\psi)}{\rho} - \frac{p_y(p_x v\cos(\psi) + p_y v\sin(\psi))}{\rho^3} = \frac{v}{\rho^3}(p_x^2 \sin\psi - p_x p_y \cos\psi)
• \frac{\partial \dot{\rho}}{\partial v} = \frac{p_x\cos(\psi) + p_y\sin(\psi)}{\rho}
• \frac{\partial \dot{\rho}}{\partial \psi} = \frac{-p_x v\sin(\psi) + p_y v\cos(\psi)}{\rho}
• \frac{\partial \dot{\rho}}{\partial \dot{\psi}} = 0

이 자코비안 행렬의 각 원소는 중요한 물리적 의미를 담고 있다. 이는 단순히 계산을 위한 도구가 아니라, 특정 측정값이 각 상태 변수에 대해 얼마나 민감한 정보(sensitivity)를 담고 있는지를 나타내는 지표이기도 하다. 예를 들어, \frac{\partial \phi}{\partial v} = 0 이라는 사실은 레이더의 방위각 측정값 \phi가 차량의 속력 v를 추정하는 데 아무런 직접적인 정보를 주지 못한다는 것을 의미한다. 반면, \frac{\partial \dot{\rho}}{\partial v} 항은 0이 아니므로, 거리 변화율 \dot{\rho} 측정값은 속력 v를 추정하는 데 직접적인 단서가 된다. 이처럼 자코비안을 분석하면 어떤 센서가 어떤 상태 변수를 추정하는 데 더 기여하는지 통찰을 얻을 수 있다.

EKF를 넘어서: 더 나은 필터를 향한 여정

EKF는 수많은 비선형 문제에 성공적으로 적용되어 왔지만, 근본적인 한계를 가지고 있다. 이 한계를 이해하는 것은 더 발전된 필터인 무향 칼만 필터(UKF)와 파티클 필터(PF)의 필요성을 이해하는 데 중요하다.

EKF의 근본적 한계: 언제 EKF는 실패하는가?

EKF는 다음과 같은 상황에서 심각한 성능 저하를 겪거나 발산할 수 있다.

• 강한 비선형성 (Strong Non-linearity): 시스템의 상태 전이나 측정 모델이 매우 비선형적일 때, 즉 함수 그래프가 급격하게 휘어질 때, 1차 테일러 급수 전개를 통한 선형 근사는 실제 함수와 너무 큰 차이를 보이게 된다. 이 선형화 오차는 매 스텝마다 누적되어 결국 필터가 엉뚱한 방향으로 가게 만든다.8
• 비가우시안 분포 (Non-Gaussian Distribution): EKF의 모든 과정은 상태의 불확실성이 단일 가우시안 분포로 표현될 수 있다는 가정에 기반한다. 하지만 초기 불확실성이 가우시안이 아니거나, 강한 비선형 변환을 거치면서 확률 분포가 여러 개의 봉우리를 갖는 다봉성(multi-modal) 분포나 한쪽으로 심하게 치우친(skewed) 분포로 변형될 경우, 단 하나의 평균과 공분산으로 이 분포를 표현하려는 EKF의 시도는 현실을 제대로 반영하지 못하게 된다.4
• 미분 불가능한 모델: 모델 함수에 if 문이나 abs() 함수처럼 미분 불가능한 지점이 포함되어 있다면, 자코비안 자체를 계산할 수 없으므로 EKF를 원천적으로 적용할 수 없다.

EKF에서 UKF, 그리고 PF로의 발전은 단순히 정확도의 점진적 향상이 아니다. 이는 비선형성을 다루는 핵심 전략의 근본적인 철학적 변화를 나타낸다.

• EKF의 철학: “모델이 너무 어려우니, 모델 자체를 근사하자.” EKF는 비선형 함수 f와 h를 직접 선형화한다. 모든 계산은 이 단순화된 선형 모델 위에서 이루어진다.1
• UKF의 철학: “모델을 근사하는 것은 부정확하다. 대신 확률 분포를 근사하자.” UKF는 비선형 함수를 건드리지 않는다. 대신, 가우시안 분포의 평균과 공분산을 완벽하게 대표하는 소수의 샘플 포인트(시그마 포인트)를 지능적으로 선택한다. 그리고 이 포인트들을 있는 그대로의 비선형 함수에 통과시킨 후, 변환된 포인트들로부터 새로운 평균과 공분산을 계산한다. 복잡한 함수를 하나의 접선으로 근사하는 것보다, 분포를 몇 개의 대표점으로 근사하는 것이 더 낫다는 발상이다.1
• PF의 철학: “왜 굳이 가우시안 분포를 가정해야 하는가? 분포를 그냥 무작위 샘플들의 집합으로 근사하자.” 파티클 필터는 확률 분포의 모양에 대한 어떠한 가정도 하지 않는다. 분포를 수많은 가중치를 가진 샘플(파티클)들의 집합으로 표현한다. 이는 가장 유연한 접근법으로, 임의의 복잡한 분포도 표현할 수 있지만 가장 큰 계산 비용을 요구한다.24

무향 칼만 필터(UKF): 자코비안 대신 시그마 포인트로

무향 칼만 필터(Unscented Kalman Filter, UKF)는 EKF의 선형화 오차 문제를 해결하기 위해 등장했다. UKF의 핵심 철학은 “비선형 함수를 근사하는 것보다, 확률 분포를 근사하는 것이 더 쉽고 정확하다“는 것이다.22

UKF의 동작 방식은 다음과 같다.

1. 시그마 포인트 생성: 현재 상태의 평균과 공분산 정보를 완벽하게 대표하는 소수의 샘플 포인트, 즉 시그마 포인트(Sigma Points)를 결정론적인 규칙에 따라 선택한다. 상태 벡터의 차원이 n일 때 보통 2n+1개의 시그마 포인트를 사용한다.26
2. 포인트 전파: 이 시그마 포인트들을 자코비안 계산 없이 그대로 실제 비선형 함수(f 또는 h)에 입력하여 변환시킨다.
3. 분포 재구성: 변환된 시그마 포인트들의 가중 평균(weighted average)과 가중 공분산(weighted covariance)을 계산하여, 변환 후의 새로운 가우시안 분포를 재구성한다.26

EKF가 분포의 평균 지점 단 하나만을 사용하여 선형화를 수행하는 반면, UKF는 분포의 ‘형태’(분산)까지 고려한 여러 개의 지점(시그마 포인트)을 사용한다. 이 덕분에 UKF는 비선형 변환 후의 실제 평균과 공분산을 EKF보다 훨씬 더 정확하게(테일러 급수 기준으로 최소 2차 항까지, 종종 3차 항까지) 근사할 수 있다.27 자코비안을 계산할 필요가 없어 복잡한 편미분 유도 과정에서 해방된다는 점은 매우 큰 실용적 장점이다.18

파티클 필터(PF): 가우시안의 굴레를 벗어난 궁극의 유연성

파티클 필터(Particle Filter, PF)는 EKF와 UKF가 여전히 고수하고 있는 ’가우시안’이라는 가정마저 완전히 버린, 가장 유연한 비선형 필터다. PF의 핵심 아이디어는 어떤 복잡한 형태의 확률 분포라도, 수많은 무작위 샘플, 즉 파티클(Particles)들의 집합으로 근사할 수 있다는 것이다.25

PF의 동작 방식은 직관적인 몬테카를로 시뮬레이션에 가깝다.

1. 초기화: 수천, 수만 개의 파티클을 상태 공간에 뿌린다. 각 파티클은 “객체가 여기에 있을 수도 있다“는 하나의 가설을 의미한다.
2. 예측 (Propagation): 모든 파티클을 상태 전이 모델 f에 따라 이동시킨다. 이때 약간의 무작위 노이즈를 추가하여 시스템의 불확실성을 반영한다.
3. 업데이트 (Weighting): 실제 측정값 \mathbf{z}_k가 들어오면, 각 파티클이 이 측정값을 얼마나 잘 설명하는지를 나타내는 가능도(likelihood)를 계산한다. 이 가능도를 기반으로 각 파티클에 가중치(weight)를 부여한다. 실제 측정값과 가까운 위치의 파티클은 높은 가중치를, 먼 곳의 파티클은 낮은 가중치를 받게 된다.
4. 리샘플링 (Resampling): 가중치가 낮은 파티클은 버리고, 가중치가 높은 파티클을 그 가중치에 비례하여 복제한다. 이는 마치 ’적자생존’처럼, 가능성이 낮은 가설은 도태시키고 유망한 가설에 더 많은 자원을 집중하는 과정이다. 이 단계를 통해 파티클 고갈(degeneracy) 문제를 해결하고 파티클들이 실제 상태 주변에 모이게 된다.24

PF는 가우시안 가정을 완전히 버렸기 때문에, 확률 분포가 두 개 이상의 봉우리를 갖는 다봉성(multi-modal) 문제나 매우 비대칭적인 분포를 갖는 문제를 유일하게 제대로 해결할 수 있다. 예를 들어, 로봇이 복도를 가다가 누군가에게 납치되어 다른 위치로 순간 이동했다고 가정해보자. 이 경우 로봇의 위치에 대한 확률 분포는 원래 있던 위치와 새로 옮겨진 위치, 두 곳에 봉우리를 갖게 된다. EKF나 UKF는 이 두 봉우리의 어중간한 중간 지점을 평균으로 계산하여 완전히 틀린 추정을 할 수 있지만, PF는 두 위치에 모두 파티클을 유지함으로써 이러한 상황을 올바르게 표현할 수 있다.6

결론: 현명한 EKF 활용을 위한 최종 조언

확장 칼만 필터는 비선형 상태 추정의 세계로 들어가는 첫 관문이자, 여전히 많은 분야에서 현역으로 활약하는 강력한 도구다. EKF의 성공적인 적용은 알고리즘 자체의 이해를 넘어, 모델링과 튜닝의 디테일에 달려 있다.

EKF 구현을 위한 핵심 체크리스트

EKF를 구현하고 디버깅할 때 다음 체크리스트를 따라가면 많은 실수를 예방할 수 있다.

1. 모델링의 정확성: 상태 전이 함수 f와 측정 함수 h가 물리적 현실을 올바르게 반영하는가? CTRV 모델의 단위(라디안 vs. 도)나 좌표계(전역 vs. 지역) 같은 기본적인 사항을 명확히 정의했는가?
2. 자코비안의 검증: 자코비안 행렬 \mathbf{F}와 \mathbf{H}를 손으로 유도한 후, 반드시 수치 미분(finite differences)을 이용해 그 결과를 검증해야 한다. 이는 EKF 구현에서 가장 흔한 버그의 원천이다.
3. 초기값의 합리성: 초기 상태 \hat{\mathbf{x}}_{0|0}와 초기 공분산 \mathbf{P}_{0|0}을 합리적으로 설정했는가? 초기 상태를 전혀 모른다면 공분산을 큰 값으로 설정하여 필터가 초기에는 측정값을 더 신뢰하도록 만들 수 있다. 하지만 너무 큰 값은 수치적 불안정성을 야기할 수 있다.
4. 노이즈 튜닝의 기술: 프로세스 노이즈 공분산 \mathbf{Q}와 측정 노이즈 공분산 \mathbf{R}은 필터의 성능을 좌우하는 가장 중요한 튜닝 파라미터다. \mathbf{R}은 보통 센서 제조사의 사양을 참고하여 설정할 수 있지만, \mathbf{Q}는 시스템 모델이 얼마나 불완전한지를 나타내므로 실험적으로 결정해야 한다. \mathbf{Q}를 크게 하면 필터가 모델 예측보다 측정값을 더 믿게 되어 반응이 빨라지지만 노이즈에 민감해지고, 작게 하면 예측을 더 믿어 결과가 부드러워지지만 실제 변화를 늦게 따라갈 수 있다. 필터의 혁신 시퀀스(\tilde{\mathbf{y}}_k)를 모니터링하며 그 통계적 특성을 분석하여 \mathbf{Q}와 \mathbf{R}을 튜닝하는 기법도 있다.
5. 특이점 처리 로직: CTRV 모델에서 선회율 \dot{\psi}이 0에 가까워지는 경우처럼, 모델에 수학적 특이점이 존재한다면 이를 회피하기 위한 분기 처리 로직이 반드시 포함되어야 한다.

구현을 위한 조언 및 라이브러리

이론을 이해했다면 직접 코드를 작성해보는 것이 최고의 학습 방법이다. numpy 라이브러리를 사용하면 EKF의 행렬 연산을 효율적으로 구현할 수 있다. 다음은 EKF 업데이트 단계의 핵심 로직을 보여주는 Python 코드 스니펫의 예시다.

# H는 상태에 따라 계산된 자코비안 행렬
# P_pred는 예측된 공분산, R은 측정 노이즈 공분산
# np는 numpy 라이브러리
S = H @ P_pred @ H.T + R

# 칼만 이득 계산
K = P_pred @ H.T @ np.linalg.inv(S)

# z는 실제 측정값, hx는 예측된 측정값 (h(x_pred))
y = z - hx

# 상태 및 공분산 업데이트
x_est = x_pred + K @ y
P_est = (np.eye(n_states) - K @ H) @ P_pred

CTRV 모델과 라이다/레이더 센서 퓨전을 구현한 완전한 형태의 Python 예제는 GitHub 등에서 쉽게 찾아볼 수 있다.28 또한, 모든 것을 밑바닥부터 구현할 필요는 없다.

FilterPy와 같은 잘 알려진 오픈소스 라이브러리는 EKF, UKF 등 다양한 필터를 구현해 놓아 사용자가 자신의 문제에 집중할 수 있도록 돕는다.30

심화 학습을 위한 참고 자료

이 문서를 통해 EKF의 기초를 다졌다면, 더 깊이 있는 학습을 위해 다음 자료들을 참고하는 것이 좋다.

• 서적:
• Sebastian Thrun, Wolfram Burgard, Dieter Fox, “Probabilistic Robotics”: 로봇 공학 관점에서 칼만 필터, EKF, UKF, 파티클 필터를 매우 직관적이고 실용적으로 설명하는 필독서다.25
• Yaakov Bar-Shalom, X. Rong Li, Thiagalingam Kirubarajan, “Estimation with Applications to Tracking and Navigation”: 추적 및 항법 분야의 바이블로, 더 깊이 있는 수학적 이론과 다양한 응용 사례를 다룬다.
• 주요 논문:
• R. E. Kalman, “A New Approach to Linear Filtering and Prediction Problems” (1960): 모든 것의 시작이 된 역사적인 논문.
• S. Julier & J. Uhlmann, “A New Extension of the Kalman Filter to Nonlinear Systems” (1997): UKF를 처음 제안한 논문.
• A. Doucet et al., “On sequential Monte Carlo sampling methods for Bayesian filtering” (2000): 파티클 필터의 이론적 기반을 다진 중요한 논문.

EKF는 완벽하지 않다. 하지만 그 한계를 명확히 이해하고, 문제의 특성에 맞게 UKF나 파티클 필터와 같은 대안을 고려할 수 있을 때, 비로소 진정한 상태 추정 전문가로 거듭날 수 있을 것이다.

참고 자료

1. Extended Kalman filter - Wikipedia, accessed July 29, 2025, https://en.wikipedia.org/wiki/Extended_Kalman_filter
2. Kalman and Extended Kalman Filters: Concept, Derivation and Properties - ResearchGate, accessed July 29, 2025, https://www.researchgate.net/publication/2888846_Kalman_and_Extended_Kalman_Filters_Concept_Derivation_and_Properties
3. Derivation of Extended Kalman Filtering and Smoothing Equations, accessed July 29, 2025, https://users.ece.cmu.edu/~byronyu/papers/derive_eks.pdf
4. Probabilistic Robotics, accessed July 29, 2025, http://ais.informatik.uni-freiburg.de/teaching/ws15/mapping/pdf/slam04-ekf.pdf
5. The Extended Kalman Filter (EKF) - Wireless Pi, accessed July 29, 2025, https://wirelesspi.com/the-extended-kalman-filter-ekf/
6. Probabilistic Algorithms in Robotics - Washington, accessed July 29, 2025, https://courses.cs.washington.edu/courses/cse599j/12sp/papers/ThrunProbRobotics-AIMagazine.pdf
7. The math behind Extended Kalman Filtering | by Sasha Przybylski - Medium, accessed July 29, 2025, https://medium.com/@sasha_przybylski/the-math-behind-extended-kalman-filtering-0df981a87453
8. An Improved Extended Kalman Filter for Radar Tracking of Satellite Trajectories - MDPI, accessed July 29, 2025, https://www.mdpi.com/2411-9660/5/3/54
9. What is the difference between EKF and UKF? - ResearchGate, accessed July 29, 2025, https://www.researchgate.net/post/What_is_the_difference_between_EKF_and_UKF
10. The Jacobian Matrix, Real Analysis II - YouTube, accessed July 29, 2025, https://www.youtube.com/watch?v=Hpbo4qYgFKA
11. trackingEKF - Extended Kalman filter for object tracking - MATLAB - MathWorks, accessed July 29, 2025, https://www.mathworks.com/help/radar/ref/trackingekf.html
12. Mathematic Formula Note of Unscented Kalman Filter with CTRV model - fevemania’s blog, accessed July 29, 2025, https://fevemania.github.io/blog/mathematic-formula-note-of-unscented-kalman-filter/
13. Comparative evaluation of Kalman filters and motion models in vehicular state estimation and path prediction | The Journal of Navigation - Cambridge University Press, accessed July 29, 2025, https://www.cambridge.org/core/journals/journal-of-navigation/article/comparative-evaluation-of-kalman-filters-and-motion-models-in-vehicular-state-estimation-and-path-prediction/8E194806C5BBE5249A351067801E9CE9
14. Extended-Kalman-Filter-CTRV.py - GitHub, accessed July 29, 2025, https://github.com/balzer82/Kalman/blob/master/Extended-Kalman-Filter-CTRV.py
15. www.mathworks.com, accessed July 29, 2025, https://www.mathworks.com/help/fusion/ref/ctrvjac.html#:~:text=Description,to%20the%20input%20state%2C%20state%20.
16. ctrvjac - Jacobian of state transition function based on constant turn-rate and velocity-magnitude motion model - MATLAB - MathWorks, accessed July 29, 2025, https://www.mathworks.com/help/fusion/ref/ctrvjac.html
17. Kalman/.ipynb_checkpoints/Extended-Kalman-Filter-CTRA-checkpoint.ipynb at master / balzer82/Kalman / GitHub, accessed July 29, 2025, https://github.com/balzer82/Kalman/blob/master/.ipynb_checkpoints/Extended-Kalman-Filter-CTRA-checkpoint.ipynb
18. Why should I still use EKF instead of UKF? - Robotics Stack Exchange, accessed July 29, 2025, https://robotics.stackexchange.com/questions/3063/why-should-i-still-use-ekf-instead-of-ukf
19. Extended Kalman Filters - MATLAB & Simulink - MathWorks, accessed July 29, 2025, https://www.mathworks.com/help/fusion/ug/extended-kalman-filters.html
20. Extended Kalman Filter (EKF) for Non Linear (Coordinate Conversion - Polar to Cartesian) Measurements and Linear Predictions, accessed July 29, 2025, https://dsp.stackexchange.com/questions/51386/extended-kalman-filter-ekf-for-non-linear-coordinate-conversion-polar-to-ca
21. 16.4 Extended Kalman Filter - Carnegie Mellon University, accessed July 29, 2025, https://www.cs.cmu.edu/~16385/s17/Slides/16.4_Extended_Kalman_Filter.pdf
22. A Comparison of Unscented and Extended Kalman Filtering for Estimating Quaternion Motion - Department of Electrical Engineering and Computer Science, accessed July 29, 2025, http://www.eecs.ucf.edu/isuelab/publications/pubs/laviola_acc2003.pdf
23. The Unscented Kalman Filter for Nonlinear Estimation - Harvard University, accessed July 29, 2025, https://groups.seas.harvard.edu/courses/cs281/papers/unscented.pdf
24. Probabilistic-Robotics-Algorithms/README.md at master - GitHub, accessed July 29, 2025, https://github.com/ChengeYang/Probabilistic-Robotics-Algorithms/blob/master/README.md
25. Probabilistic Algorithms in Robotics - Sebastian Thrun, accessed July 29, 2025, http://robots.stanford.edu/papers/thrun.probrob.pdf
26. Extended and Unscented Kalman Filter Algorithms for Online State Estimation - MathWorks, accessed July 29, 2025, https://www.mathworks.com/help/ident/ug/extended-and-unscented-kalman-filter-algorithms-for-online-state-estimation.html
27. LiDAR and Radar Sensor Fusion using Unscented Kalman Filter | by Nikhil Nair | Medium, accessed July 29, 2025, https://medium.com/@nikhilnair8490/lidar-and-radar-sensor-fusion-using-unscented-kalman-filter-5b20de0ab1d1
28. sharathsrini/Extended-Kalman-Filter-for-Sensor-Fusion - GitHub, accessed July 29, 2025, https://github.com/sharathsrini/Extended-Kalman-Filter-for-Sensor-Fusion
29. A2Amir/Extended-Kalman-Filter-for-Sensor-Fusion-Radar-and-Lidar - GitHub, accessed July 29, 2025, https://github.com/A2Amir/Extended-Kalman-Filter-for-Sensor-Fusion-Radar-and-Lidar
30. extended-kalman-filter / GitHub Topics, accessed July 29, 2025, https://github.com/topics/extended-kalman-filter
31. Probabilistic Robotics, accessed July 29, 2025, https://gaoyichao.com/Xiaotu/resource/refs/PR.MIT.en.pdf